#include <vector>
using std::vector;
#include <stdio.h>

#define PRECISION 0

template<typename T>
using Matrix = vector<vector<T>>;


template<typename T>
Matrix<T> matrixMultiply(Matrix<T> const &m1, Matrix<T> const &m2) {
    int mr = m1.size(); //结果行数
    int mc = m2[0].size();  //结果列数
    int rc = m2.size();  //过程行/列
    Matrix<T> mtx(mr, vector<T>(mc,0));
    for(int i=0; i<mr; i++) { 
        for(int j=0; j<mc; j++) { 
            for(int k=0; k<rc; k++) { 
                mtx[i][j] += m1[i][k]*m2[k][j];
            }               
        }
    }
    return mtx;
}


/********************************
   00 01 02   x   x'
   10 11 12 * y = y' 
   20 21 22   z   z'

*********************************/
template <typename T>
void inputCoordinateTransformation(Matrix<T> const &ctm, vector<T> &crd) {
    Matrix<T>crdm(3, vector<T>(1));  //坐标向量矩阵化(行转列)
    for(int i=0; i<3; i++) 
        crdm[i][0] = crd[i];
    Matrix<T> m3 = matrixMultiply(ctm, crdm);
    for(int i=0; i<3; i++)
        crd[i] = m3[i][0];
         
}  


void printCrd(vector<double> const &crd) {
    printf("Crd: x:%.*f, y:%.*f, z:%.*f\n", PRECISION, crd[0], PRECISION, crd[1], PRECISION, crd[2]);
}

void printCtm(Matrix<double> const &ctm) {
    printf("CTM:\n");
    for(int i=0; i<3; i++) {
        for(int j=0; j<3; j++) {
            printf("%.*f ", PRECISION, ctm[i][j]);
        }
        printf("\n");
    }
}


int main(void) {
    vector<double> crd{100, 200, 0};
    Matrix<double> ctm {
        {1, 0, 0},
        {0, 1, 0},
        {0, 0, 1}
    };
    inputCoordinateTransformation(ctm, crd);

    printf("Normal:\n"); //Rotate 0
    printCtm(ctm);
    printCrd(crd);

    puts("\n");    
    
    crd = {100, 200, 0};
    ctm = {
        {0, -1, 1},
        {1, 0, 0},
        {0, 0, 1}
    };
    inputCoordinateTransformation(ctm, crd);
    printf("Left:\n"); //Rotate 90
    printCtm(ctm);
    printCrd(crd);

    puts("\n");    

    crd = {100, 200, 0};
    ctm = {
        {-1, 0, 1},
        {0, -1, 1},
        {0, 0, 1}
    };
    inputCoordinateTransformation(ctm, crd);
    printf("Invert:\n"); //Rotate 180
    printCtm(ctm);
    printCrd(crd);

    puts("\n");    

    crd = {100, 200, 0};
    ctm = {
        {0, 1, 0},
        {-1, 0, 1},
        {0, 0, 1}
    };
    inputCoordinateTransformation(ctm, crd);
    printf("Right:\n"); //Rotate 270
    printCtm(ctm);
    printCrd(crd);

    return 0;
}
